#include <iostream>
// For disable PCL complile lib, to use PointXYZIR    
//#define PCL_NO_PRECOMPILE

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/passthrough.h>

#include <velodyne_pointcloud/point_types.h>

//5太小会分割掉很多障碍物,10勉强可以
// #define RESOLUTION 10


#define VPoint pcl::PointXYZ

// using eigen lib
#include <Eigen/Dense>

using Eigen::MatrixXf;
using Eigen::JacobiSVD;
using Eigen::VectorXf;

pcl::PointCloud<VPoint>::Ptr g_seeds_pc(new pcl::PointCloud<VPoint>());
pcl::PointCloud<VPoint>::Ptr g_ground_pc(new pcl::PointCloud<VPoint>());
pcl::PointCloud<VPoint>::Ptr g_not_ground_pc(new pcl::PointCloud<VPoint>());

pcl::PointCloud<VPoint>::Ptr pub_ground(new pcl::PointCloud<VPoint>());
pcl::PointCloud<VPoint>::Ptr pub_noground(new pcl::PointCloud<VPoint>());


bool point_cmp(VPoint a, VPoint b){
    return a.z<b.z;
}

class GroundPlaneFit{
public:
    GroundPlaneFit();
private:
    ros::NodeHandle node_handle_;
    ros::Subscriber points_node_sub_;
    ros::Publisher ground_points_pub_;
    ros::Publisher groundless_points_pub_;
    // ros::Publisher all_points_pub_;

    std::string point_topic_;
    std::string input_topic;
    int RESOLUTION;

    int sensor_model_;
    double sensor_height_;
    int num_seg_;
    int num_iter_;
    int num_lpr_;
    double th_seeds_;
    double th_dist_;


    void velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);
    void estimate_plane_(void);
    void extract_initial_seeds_(const pcl::PointCloud<VPoint>::Ptr p_sorted);
    void main_loop(const pcl::PointCloud<VPoint>::Ptr ptr_in);

    // Model parameter for ground plane fitting
    // The ground plane model is: ax+by+cz+d=0
    // Here normal:=[a,b,c], d=d
    // th_dist_d_ = threshold_dist - d 
    float d_;
    MatrixXf normal_;
    float th_dist_d_;
};    

/*
    @brief Constructor of GPF Node.
    @return void
*/
GroundPlaneFit::GroundPlaneFit():node_handle_("~"){
	
    // Init ROS related
    ROS_INFO("Inititalizing Ground Plane Fitter...");
   

    node_handle_.getParam("input_topic", input_topic);
    node_handle_.getParam("RESOLUTION", RESOLUTION);

    // node_handle_.param<std::string>("point_topic", point_topic_, "/velodyne_points");
    // node_handle_.param<std::string>("point_topic", point_topic_, "/kitti/velo/pointcloud");
    // ROS_INFO("Input Point Cloud: %s", input_topic.c_str());
    // ROS_INFO("RESOLUTION: %d", RESOLUTION);

    node_handle_.param("sensor_model", sensor_model_, 64);
    // ROS_INFO("Sensor Model: %d", sensor_model_);

    node_handle_.getParam("sensor_height_", sensor_height_);
    // node_handle_.param("sensor_height", sensor_height_, 1.73);
    ROS_INFO("Sensor Height: %f", sensor_height_);

    node_handle_.param("num_seg", num_seg_, 1);
    // ROS_INFO("Num of Segments: %d", num_seg_);

    node_handle_.param("num_iter", num_iter_, 3);
    // ROS_INFO("Num of Iteration: %d", num_iter_);

    node_handle_.param("num_lpr", num_lpr_, 20);
    // ROS_INFO("Num of LPR: %d", num_lpr_);

    node_handle_.param("th_seeds", th_seeds_, 1.2);
    // ROS_INFO("Seeds Threshold: %f", th_seeds_);

    node_handle_.getParam("th_dist_", th_dist_);
    // node_handle_.param("th_dist", th_dist_, 0.15);
    ROS_INFO("Distance Threshold: %f", th_dist_);

    // Listen to velodyne topic
    points_node_sub_ = node_handle_.subscribe(input_topic, 10, &GroundPlaneFit::velodyne_callback_, this);
    
    // Publish Init
    std::string no_ground_topic, ground_topic;
    node_handle_.param<std::string>("no_ground_point_topic", no_ground_topic, "/points_no_ground");
    // ROS_INFO("No Ground Output Point Cloud: %s", no_ground_topic.c_str());
    node_handle_.param<std::string>("ground_point_topic", ground_topic, "/points_ground");
    // ROS_INFO("Only Ground Output Point Cloud: %s", ground_topic.c_str());
    groundless_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(no_ground_topic, 2);
    ground_points_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>(ground_topic, 2);

    //all_points_pub_ =  node_handle_.advertise<sensor_msgs::PointCloud2>("/all_points", 2);

    ROS_INFO("[1]+running goundplanefit_node");
}


void GroundPlaneFit::estimate_plane_(void){
    // Create covarian matrix in single pass.
    // TODO: compare the efficiency.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean;
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
    // Singular Value Decomposition: SVD
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2));
    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose()*seeds_mean)(0,0);
    // set distance threhold to `th_dist - d`
    th_dist_d_ = th_dist_ - d_;
 
    // return the equation parameters
}


void GroundPlaneFit::extract_initial_seeds_(const pcl::PointCloud<VPoint>::Ptr p_sorted){
    // LPR is the mean of low point representative
    double sum = 0;
    int cnt = 0;
    // Calculate the mean height value.
    for(size_t i=0;i<p_sorted->points.size() && cnt<num_lpr_;i++){
        sum += p_sorted->points[i].z;
        cnt++;
    }
    double lpr_height = cnt!=0?sum/cnt:0;// in case divide by 0
    g_seeds_pc->clear();
    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_
    for(size_t i=0;i<p_sorted->points.size();i++){
        if(p_sorted->points[i].z < lpr_height + th_seeds_){
            g_seeds_pc->points.push_back(p_sorted->points[i]);
        }
    }
    // return seeds points
}

//by tcx.

void GroundPlaneFit::main_loop(const pcl::PointCloud<VPoint>::Ptr ptr_in){
    extract_initial_seeds_(ptr_in);
    g_ground_pc = g_seeds_pc;
    
    pcl::PointCloud<VPoint> laserCloudIn_org;
    laserCloudIn_org = *ptr_in;
    // 5. Ground plane fitter mainloop
    for(int i=0;i<num_iter_;i++){
        estimate_plane_();
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(laserCloudIn_org.points.size(),3);
        int j =0;
        for(auto p:laserCloudIn_org.points){
            points.row(j++)<<p.x,p.y,p.z;
        }
        // ground plane model
        VectorXf result = points*normal_;
        // threshold filter
        for(int r=0;r<result.rows();r++){
            if(result[r]<th_dist_d_){
                g_ground_pc->points.push_back(laserCloudIn_org[r]);
            }else{
                g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
        }
    }
    *pub_ground += *g_ground_pc;
    *pub_noground += *g_not_ground_pc;
}


/*
    @brief Velodyne pointcloud callback function. The main GPF pipeline is here.
    PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud
    ->error points removal -> extract ground seeds -> ground plane fit mainloop
*/
void GroundPlaneFit::velodyne_callback_(const sensor_msgs::PointCloud2ConstPtr& in_cloud_msg){
    // 1.Msg to pointcloud

    /**多平面分割  by leox **/
    // 直通滤波
    pcl::PointCloud<VPoint>::Ptr laserCloudInPtr(new pcl::PointCloud<VPoint>);
    pcl::fromROSMsg(*in_cloud_msg, *laserCloudInPtr);

    pcl::PassThrough<VPoint> pass_x;
	pass_x.setInputCloud(laserCloudInPtr);
	pass_x.setFilterFieldName("x");
	pass_x.setFilterLimits(-200, 200);
	pass_x.filter(*laserCloudInPtr);

    pcl::PassThrough<VPoint> pass_y;
    pass_y.setInputCloud(laserCloudInPtr);
    pass_y.setFilterFieldName("y");
    pass_y.setFilterLimits(-100, 100);
    pass_y.filter(*laserCloudInPtr);
    
    pcl::PassThrough<pcl::PointXYZ> pass_z;
    pass_z.setInputCloud(laserCloudInPtr);
    pass_z.setFilterFieldName("z");
    pass_z.setFilterLimits((-1.05 * sensor_height_), 0.5);
    pass_z.filter(*laserCloudInPtr);


    // 2.Sort on Z-axis value.
    sort(laserCloudInPtr->points.begin(),laserCloudInPtr->end(),point_cmp);

    // 3.Error point removal
    // As there are some error mirror reflection under the ground, 
    // here regardless point under 1.2* sensor_height
    // Sort point according to height, here uses z-axis in default
    /*//使用直通滤波直接去除低于传感器高度的点
    pcl::PointCloud<VPoint>::iterator it = laserCloudInPtr->points.begin();
    for(size_t i=0;i<laserCloudInPtr->points.size();i++){
        if(laserCloudInPtr->points[i].z < -1.2*sensor_height_){
            it++;
        }else{
            break;
        }
    }
    laserCloudInPtr->points.erase(laserCloudInPtr->points.begin(),it);
    */
    
    // 对整个场景分割
    main_loop(laserCloudInPtr);


    // by tcx.
    /*
    // 划分网格，对每个网格内进行拟合
    // TODO：KITTI数据集的包会报错，段错误（核心已转出）,test.bag可以用，能够分割多个地平面
    std::vector<std::vector<pcl::PointCloud<VPoint>::Ptr>> 
        segment_pc_array( 50*2 / RESOLUTION, std::vector<pcl::PointCloud<VPoint>::Ptr>(20*2 / RESOLUTION));
        // segment_pc_array( 100, std::vector<pcl::PointCloud<VPoint>::Ptr>(40) );
    for (size_t i = 0; i < segment_pc_array.size(); i++){
        for (size_t j = 0; j < segment_pc_array[i].size(); j++){
            pcl::PointCloud<VPoint>::Ptr temp(new pcl::PointCloud<VPoint>);
            segment_pc_array[i][j] = temp;
        }
    }

    for (size_t i = 0; i < laserCloudInPtr->points.size(); i++){
        segment_pc_array[int((laserCloudInPtr->points[i].x +50) / RESOLUTION)]
                        [int((laserCloudInPtr->points[i].y +20) / RESOLUTION)]
            ->points.push_back(laserCloudInPtr->points[i]);
    }

    for (size_t i = 0; i < segment_pc_array.size(); i++){
        for (size_t j = 0; j < segment_pc_array[i].size(); j++){
            main_loop(segment_pc_array[i][j]);
            
        }
    }
    */
    

    // publish ground points
    sensor_msgs::PointCloud2 ground_msg;
    pcl::toROSMsg(*pub_ground, ground_msg);
    ground_msg.header.stamp = in_cloud_msg->header.stamp;
    ground_msg.header.frame_id = in_cloud_msg->header.frame_id;
    ground_points_pub_.publish(ground_msg);
    
    // publish not ground points
    sensor_msgs::PointCloud2 groundless_msg;
    pcl::toROSMsg(*pub_noground, groundless_msg);
    groundless_msg.header = in_cloud_msg->header;
    groundless_msg.header.frame_id = in_cloud_msg->header.frame_id;
    groundless_points_pub_.publish(groundless_msg);
    
    pub_ground->clear();
    pub_noground->clear();


}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "GroundPlaneFit");
    GroundPlaneFit node;
    ros::spin();

    return 0;

}
